#include "servo.h"

uint32_t servo_set_angle(TIM_HandleTypeDef *htim, uint32_t channel, uint32_t angle) 
{
    if (angle < SERVO_MIN_ANGLE || angle > SERVO_MAX_ANGLE)
        return -1;
    __HAL_TIM_SET_COMPARE(htim, channel, ANGLE_TO_COMPARE(angle));
    return 0;
}

uint32_t servo_get_angle(TIM_HandleTypeDef *htim, uint32_t channel)
{
    uint32_t compare = __HAL_TIM_GET_COMPARE(htim, channel);
    if (compare <= SERVO_MIN_COMPARE)
        return SERVO_MIN_ANGLE;
    else if (compare >= SERVO_MAX_COMPARE)
        return SERVO_MAX_COMPARE;
    else
        return COMPARE_TO_ANGLE(compare);
}